#include "MutuleCanModule.h"

MutuleCanModule::MutuleCanModule(const std::string &interface)
{
    can_module=std::make_shared<CanModule>(interface);
    auto read_back = std::bind(&MutuleCanModule::read_message, this, std::placeholders::_1);
    can_module->set_read_callback(read_back);
    
}
void MutuleCanModule::read_message(void *arg)
{
    Buffer *m_read_buffer = static_cast<Buffer *>(arg);
    uint8_t chgr_idx = 0;
    can_id_t can_id;
    while (m_read_buffer->has_data())
    {

        can_frame frame = {0};
        int len = m_read_buffer->read((uint8_t *)&frame, sizeof(can_frame));
        if (len!=sizeof(can_frame))
        {
            return;
        }
        can_id.data = frame.can_id;
        if (can_id.compts.buf_flag > 0)
        {
            if (frame.can_dlc != 8)
            {
                return;
            }

            if (can_id.compts.frame_idx == 0)
            {
                multiframe_start(frame);
            }
            else
            {
                multiframe_load_data(can_id.compts.frame_idx, frame);
            }
        }
        else
        {
            if (can_id.compts.frame_idx > 0) // If it's the last frame of multi-frame message
            {
                multiframe_load_last_data(can_id.compts.frame_idx, frame);
                memset(&m_mutule_can_state, 0, sizeof(multe_can_state));
            }
        }
    }
}
void MutuleCanModule::multiframe_start(can_frame frame)
{
    m_mutule_can_state.start_flag = 1;
    m_mutule_can_state.exp_frame_idx = 1;
    frames.push(frame);
}
uint8_t MutuleCanModule::multiframe_load_data(uint8_t frame_idx, can_frame frame)
{
    if (m_mutule_can_state.start_flag == 0)
    {
        return 0;
    }

    if (frame_idx != m_mutule_can_state.exp_frame_idx)
    {
        return 0;
    }
    frames.push(frame); // Store subsequent frames
    m_mutule_can_state.exp_frame_idx++;

    return 1;
}
uint8_t MutuleCanModule::multiframe_load_last_data(uint8_t frame_idx, can_frame frame)
{
    uint8_t temp = 0;

    if (frame.can_dlc == 0)
    {
        return 0;
    }
    if (m_mutule_can_state.start_flag == 0)
    {
        return 0;
    }
    if (frame_idx != m_mutule_can_state.exp_frame_idx)
    {
        return 0;
    }

    if (frame.can_dlc > 0)
    {
        frames.push(frame);      // Store the final frame
        uint8_t full_data[1024]; // Array to hold all the concatenated data
        uint16_t total_len = 0;
        // Combine data from all frames
        while (!frames.empty())
        {
            can_frame temp_frame = frames.front();
            frames.pop();

            // Ensure that the total data length does not exceed the array size
            if (total_len + temp_frame.can_dlc <= sizeof(full_data))
            {
                memcpy(&full_data[total_len], temp_frame.data, temp_frame.can_dlc);
                total_len += temp_frame.can_dlc;
            }
            else
            {
                printf("Error: Data exceeds buffer size\n");
                break;
            }
        }
        temp = crc8_calc(full_data, total_len);
        printf("%s ",full_data);
        if (temp != full_data[total_len])
        {
            printf("Error: crc error\n");
            return 0;
        }
    }

    return 1;
}

void MutuleCanModule::mutule_write_data(uint32_t canId, uint8_t *data, uint16_t data_len)
{
    can_id_t can_id;
    int wr_idx = 0;
    can_id.data = 0;
    can_frame frame = {0};
    if (data_len <= 8)
    {
        can_id.compts.src_addr = 0;
        can_id.compts.dst_addr = 0;
        can_id.compts.cmd = 0;
        can_id.compts.frame_idx = 0;
        can_id.compts.buf_flag = 0;
        frame.can_id = can_id.data;
        frame.can_dlc = data_len;
        memcpy(&frame, data, data_len);
    }
    else
    {
        uint16_t frame_num = data_len + 1;
        uint16_t i, data_num_cntr = 0;
        uint8_t temp = 0, last_frame_data_num = 0;

        if (frame_num % 8)
        {
            last_frame_data_num = frame_num % 8;
            frame_num /= 8;
            frame_num++;
        }
        else
        {
            frame_num /= 8;
            last_frame_data_num = 8;
        }

        temp = crc8_calc(data, data_len);

        for (i = 0; i < frame_num; i++)
        {
            can_id.compts.src_addr = 0;
            can_id.compts.dst_addr = 0;
            can_id.compts.cmd = 0;
            can_id.compts.frame_idx = i;

            if (i == (frame_num - 1))
            {
                can_id.compts.buf_flag = 0;
                if (last_frame_data_num > 1)
                {

                    memcpy(frame.data, data + data_num_cntr, data_len - data_num_cntr);
                    frame.can_dlc = data_len - data_num_cntr;
                }
                frame.data[data_len - data_num_cntr] = temp;
            }
            else
            {
                can_id.compts.buf_flag = 1;
                memcpy(frame.data, data + data_num_cntr, 8);
                frame.can_dlc = 8;
                data_num_cntr += 8;
            }
            frame.can_id = can_id.data;
            can_module->write_message_frame(frame);
        }
    }
}
